#include <Arduino.h>

HardwareSerial mySerial(2);

uint8_t frame[28];       // 接收缓冲
uint8_t index = 0;       // 当前字节索引

float convertIEEE754ToFloat(uint8_t* byteArray) {
  float result;
  memcpy(&result, byteArray, sizeof(result));
  return result;
}

void setup() {
  Serial.begin(115200);
  mySerial.begin(115200, SERIAL_8N1, 16, 17);
  Serial.println("ESP32 - 正在等待帧数据...");
}

void loop() {
  while (mySerial.available()) {
    uint8_t ch = mySerial.read();

    // 添加到缓冲区
    if (index < 28) {
      frame[index++] = ch;
    } else {
      // 缓冲满了，向前滑动一位，扔掉最旧的
      memmove(frame, frame + 1, 27);
      frame[27] = ch;
    }

    // 判断是否组成完整帧（长度28，且包头帧尾符合）
    if (index >= 28 &&
        frame[0] == 0x0D && frame[1] == 0x0A &&
        frame[26] == 0x0A && frame[27] == 0x0D) {
      
      float zangle = convertIEEE754ToFloat(&frame[2]);
      float xangle = convertIEEE754ToFloat(&frame[6]);
      float yangle = convertIEEE754ToFloat(&frame[10]);
      float pos_x = convertIEEE754ToFloat(&frame[14]);
      float pos_y = convertIEEE754ToFloat(&frame[18]);
      float w_z   = convertIEEE754ToFloat(&frame[22]);

      Serial.println("=== ✅ 接收到一帧完整数据 ===");
      Serial.print("zangle: "); Serial.println(zangle);
      Serial.print("xangle: "); Serial.println(xangle);
      Serial.print("yangle: "); Serial.println(yangle);
      Serial.print("pos_x : "); Serial.println(pos_x);
      Serial.print("pos_y : "); Serial.println(pos_y);
      Serial.print("w_z   : "); Serial.println(w_z);
      Serial.println("================================");

      // 接收完一帧，准备下一帧，从0开始（丢弃当前帧）
      index = 0;
    }
  }
}
